#ifndef IMU_PACKET_FINDER_HPP_
#define IMU_PACKET_FINDER_HPP_

/*****************************************************************************
 ** Includes
 *****************************************************************************/

#include <iomanip>
#include <StandardCplusplus.h>
#include <deque>

using namespace std;
/*****************************************************************************
 ** Namespaces
 *****************************************************************************/
/**
data struct
起始字节
字节数
功能字节
数据体
检验字节
结束字节
5.1.3.1 起始字节
报文起始字节,共两个字节,第一个字节为 0x7A,第二个字节为 0x7B。
示例:传感器解算后的姿态角、倾角报文内容为 
header:      0x7A 0x7B 
length:      0x12 
type:        0xB1 
航向角yaw:    0x20 0x0A 0x5B
俯仰角:       0x00 0x4D 0x84 
横滚角:       0x80 0x09 0x52 
倾斜角:       0x00 0x01 0x78 
ASV 解算速率: 0x00 0x78 
check code:  0xC1 
end:         0xBB

**/
class ImuPacketFinderBase
{
public:
  typedef deque<unsigned char> BufferType;

  enum packetFinderState
  {
    clearBuffer = 0,
    waitingForStx,
    waitingForDataSize,
    waitingForPayloadSize,
  };
  enum packetFinderType
  {
    none = 0,
    pose = 1,    
  };  
  enum packetFinderState state;

  unsigned int size_stx;
  //unsigned int size_etx;
  unsigned int size_length_field;  
protected:


  bool variable_size_payload;
  unsigned int size_max;
  unsigned int size_payload;
  unsigned int size_checksum_field;

  BufferType STX;
  BufferType ETX;
  BufferType buffer;

  bool verbose;

public:
  ImuPacketFinderBase(); /**< Default constructor. Use with configure(). **/

  virtual ~ImuPacketFinderBase() {};

  void configure();
  void clear();
  void enableVerbose();
  virtual bool update(const unsigned char * incoming, unsigned int numberOfIncoming);
  virtual bool checkSum();
  unsigned int numberOfDataToRead();
  void getBuffer(BufferType & bufferRef);
  void getPayload(BufferType & bufferRef);
  // int16_t getYaw();

protected:
  bool WaitForStx(const unsigned char datum);
  bool waitForDataSize(const unsigned char size);
  bool waitForPayloadSize(const unsigned char * incoming, unsigned int numberOfIncoming, bool & foundPacket);  
};

#endif /* IMU_PACKET_FINDER_HPP_ */
